#include <iostream>
#include <fstream>
#include <vector>
using namespace std;

#include <dalelib/maths.h>
#include <dalelib/graphics.h>
#include <dalelib/graphics/skeleton.h>

namespace dalelib
{

//*************************************************************************


Skeleton::~Skeleton()
{
	for(unsigned int i=0; i<_bone_list.size(); ++i) delete _bone_list[i];
}

void Skeleton::init()
{
//	for(int i=0; i<50; ++i) update(); // so crap, I know
	setGlobalMatRestInv();
}

void Skeleton::update()
{
	for(unsigned int i=0; i<_bone_list.size(); ++i)
	{
		Bone* current_bone = _bone_list[i];
		current_bone->update();
		_transforms[i] = current_bone->getRestToPoseMat().transpose();
	}
}

void Skeleton::setGlobalMatRestInv()
{
	for(unsigned int i=0; i<_bone_list.size(); ++i)
	{
		_bone_list[i]->setGlobalMatRestInv();
	}
}

void Skeleton::draw(const int _CurrentBone)
{
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
	glDisable(GL_BLEND);
	glDisable(GL_TEXTURE_2D);
    glEnable(GL_COLOR_MATERIAL);
    glColorMaterial(GL_FRONT, GL_DIFFUSE);
	glEnable(GL_NORMALIZE);

	for(unsigned int i=0; i<_bone_list.size(); ++i)
	{
		_bone_list[i]->draw(i==_CurrentBone);
	}
    glShadeModel(GL_SMOOTH);
	glColor4f(1,1,1,1);
}

void Skeleton::load(const string _FileName)
{
	ifstream fin( _FileName.c_str() );
	if( !fin ) { cout << "Error: couldn't load skeleton: " << _FileName << endl; return; }

	vector<TempBoneStruct> temp_bones;

	// *** load int bone data ***
	{
		string str;
		do
		{
			fin >> str;
			if( !str.compare("BONE") )
			{
				TempBoneStruct temp_bone;
				fin >> temp_bone.name;
				fin >> temp_bone.start[0] >> temp_bone.start[1] >> temp_bone.start[2];
				fin >> temp_bone.end[0] >> temp_bone.end[1] >> temp_bone.end[2];
				fin >> temp_bone.parent_name;
				fin >> temp_bone.amin[0] >> temp_bone.amax[0] >> temp_bone.amin[1] >> temp_bone.amax[1] >> temp_bone.amin[2] >> temp_bone.amax[2];

				int parent_index = -1;
				for(int i=0; i<temp_bones.size(); ++i)
				{
					if( !temp_bone.parent_name.compare(temp_bones[i].name) )
					{
						temp_bone.parent_index = i;
					}
				}
				temp_bones.push_back(temp_bone);
				cout << "bone " << temp_bone.name << " " << temp_bone.start[0] << " " << temp_bone.start[1] << " " << temp_bone.start[2] << "   " << temp_bone.end[0] << " " << temp_bone.end[1] << " " << temp_bone.end[2] << "   " << temp_bone.parent_name << " " << temp_bone.parent_index << endl;
			}
			str = "";
		} while( !fin.eof() );
		fin.close();
	}

	// *** construct bones from bones ***
	{
	    _bone_list.resize(temp_bones.size());
		for(int i=0; i<temp_bones.size(); ++i)
		{
			Bone *b, *parentptr = 0;
			v3 start = temp_bones[i].start;
			if( _bone_map.find(temp_bones[i].parent_name) != _bone_map.end() )
			{
				parentptr = _bone_map[temp_bones[i].parent_name];
				if(start.len2()<0.001f) start = temp_bones[temp_bones[i].parent_index].end;
			}

			Bone* bone = new Bone( temp_bones[i].name, parentptr, start, temp_bones[i].end, temp_bones[i].amin, temp_bones[i].amax );
			bone->getID() = i;
			_bone_list[i] = bone;
            _bone_map[temp_bones[i].name] = bone;
//			cout << "new bone: " << parentptr << "     " << start[0] << " " << start[1] << " " << start[2] << "       " << temp_bones[i].pos[0] << " " << temp_bones[i].pos[1] << " " << temp_bones[i].pos[2] << endl;
		}
	}

	// *** extra setup ***
	{
		_transforms.resize(_bone_list.size());
	}

}

void Skeleton::loadMarionetteStrings(const string _FileName)
{
	ifstream fin( _FileName.c_str() );
	if( !fin ) { cout << "Error: couldn't load marionette strings: " << _FileName << endl; return; }

	string str;
	do
	{
		fin >> str;
		if( !str.compare("STRING") )
		{
			string n, t; // name and targetname
			float s;
			fin >> n >> t >> s;
			_strings.push_back(MarionetteString(n,t,s));
			cout << "STRING: " << n << " -> " << t << "  " << s << endl;
		}
		str = "";
	} while( !fin.eof() );
	fin.close();

}

void Skeleton::scale(const v3 _Scale)
{
    for(unsigned int i=0; i<_bone_list.size(); ++i)
    {
		if( _bone_list[i]->getParent() )
		{
			_bone_list[i]->getPos() *= _Scale;
//			(*i).second->getLength() *= _Scale;
			_bone_list[i]->getLength() *= _Scale[0]; // need more sophisticated anisotropic scaling based on dot products with axis
		}
	}
}

void Skeleton::translate(const v3 _Vec)
{
    for(unsigned int i=0; i<_bone_list.size(); ++i)
	{
		if( !_bone_list[i]->getParent() )
		{
			_bone_list[i]->getPos() += _Vec;
		}
	}
}

void Skeleton::inverseKinematicPull(const string _BoneName,v3 _Target,const float _Speed,const int _MaxNumLinks)
{
    if(_bone_map.find(_BoneName)==_bone_map.end()) return;

	Bone* bone = _bone_map[_BoneName];
	v3 tip = bone->getBoneEnd();
/*
	glDisable(GL_DEPTH_TEST);
	glDisable(GL_LIGHTING);
	glLineWidth(1.f);
	glColor4f(0,1,0,1);
	glBegin(GL_LINES);
	    glVertex3fv(tip.ptr());
	    glVertex3fv(_Target.ptr());
	glEnd();
	glColor3f(1,1,1);
//*/

	v3 totarget = _Target-tip;
	if( !bone->getParent() )
	{
		bone->getPos() += totarget*_Speed;
	}
	else
	{
		// only rotate joints that are ancestors to current bone
		vector<bool> mask(_bone_list.size(),false);
		{
			int finished = _MaxNumLinks;
			Bone* bb = bone;
			while((finished--)>0)
			{
				mask[bb->getID()] = true;
				if( bb->getParent() )
				{
					bb = bb->getParent();
				}
				else
				{
                    finished = 0;
				}
			}
		}


        for(unsigned int i=0; i<_bone_list.size(); ++i)
		{
			Bone* current_bone = _bone_list[i];
			if( mask[i] )
			{
//*
                const v3 base = current_bone->getGlobalMat().getTranslationPart();
				const v3 totip = (tip-base).unit();
				const v3 totarget = (_Target-base).unit();
				v3 crosstiptarget = v3::cross(totip,totarget);
				if(v3::dot(crosstiptarget,crosstiptarget)>0.1f) crosstiptarget.normalise();

				v3& ang = current_bone->getAng();
				float dir, dang;

				const v3 xaxis = current_bone->getGlobalMat().x().xyz();
				dir = v3::dot(xaxis,crosstiptarget);
				dang = acos(v3::dot(totip,totarget));
				ang[0] += dir*dang*_Speed;

				const v3 yaxis = current_bone->getGlobalMat().y().xyz();
				dir = v3::dot(yaxis,crosstiptarget);
				dang = acos(v3::dot(totip,totarget));
				ang[1] += dir*dang*_Speed;

				const v3 zaxis = current_bone->getGlobalMat().z().xyz();
				dir = v3::dot(zaxis,crosstiptarget);
				dang = acos(v3::dot(totip,totarget));
				ang[2] += dir*dang*_Speed;
/*/
				v3 totip = tip-current_bone->getGlobalMat().getTranslationPart();
				v3& ang = current_bone->getAng();

				v3 xaxis = current_bone->getGlobalMat().x().xyz();
				v3 xmove = v3::cross(totip,xaxis);
				ang[0] -= v3::dot(xmove,totarget)*_Speed;

				v3 yaxis = current_bone->getGlobalMat().y().xyz();
				v3 ymove = v3::cross(totip,yaxis);
				ang[1] -= v3::dot(ymove,totarget)*_Speed*0.1f;

				v3 zaxis = current_bone->getGlobalMat().z().xyz();
				v3 zmove = v3::cross(totip,zaxis);
				ang[2] -= v3::dot(zmove,totarget)*_Speed*0.1f;
//*/

				ang.clamp(current_bone->getMinAngleLimits(),current_bone->getMaxAngleLimits());

                for(unsigned int j=0; j<_bone_list.size(); ++j)
                {
					if( mask[j] ) _bone_list[j]->update();
                }
			}
//			update();
		}
	}
}

} // namespace dalelib
